#include <iostream>
#include <math.h>
#include "d_Math.h"

using namespace d_Math;

d_Mat4f::d_Mat4f()
{
	for(int i=0; i<4; i++) 
		for(int j=0; j<4; j++)
			p[i][j] = 0;
}

void d_Mat4f::make_diag()
{
	make_diag(1);
}

void d_Mat4f::make_hrot(const d_Vec3f &axis, float theta)	
{
	float		s;
	d_Vec4f		q;
	
	theta /= 2.0;
	s = sin(theta);
	
	q[0] = s * axis[0];
	q[1] = s * axis[1];
	q[2] = s * axis[2];
	q[3] = cos(theta);
	
	make_hrot(q);
}

void d_Mat4f::make_hrot(const d_Vec4f &q)
{
	float i2 =  2 * q[0],
			j2 =  2 * q[1],
			k2 =  2 * q[2],
			ij = i2 * q[1],
			ik = i2 * q[2],
			jk = j2 * q[2],
			ri = i2 * q[3],
			rj = j2 * q[3],
			rk = k2 * q[3];

	make_diag();
				
	i2 *= q[0];
	j2 *= q[1];
	k2 *= q[2];
/*
#if VL_ROW_ORIENT
	row[0][0] = 1 - j2 - k2;  row[0][1] = ij + rk   ;  row[0][2] = ik - rj;
	row[1][0] = ij - rk    ;  row[1][1] = 1 - i2- k2;  row[1][2] = jk + ri;
	row[2][0] = ik + rj	   ;  row[2][1] = jk - ri   ;  row[2][2] = 1 - i2 - j2;
#else*/
	p[0][0] = 1 - j2 - k2;  p[0][1] = ij - rk   ;  p[0][2] = ik + rj;
	p[1][0] = ij + rk    ;  p[1][1] = 1 - i2- k2;  p[1][2] = jk - ri;
	p[2][0] = ik - rj;  p[2][1] = jk + ri   ;  p[2][2] = 1 - i2 - j2;
//#endif

}

void d_Mat4f::make_diag(float value)
{
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			if(i==j)
				p[i][j]=value;
			else
				p[i][j]=0;
}

void d_Mat4f::print()
{
	std::cout << "[";
	for (int i=0; i<3; i++) 
	{
		std::cout << "[" << p[i][0] << "," << p[i][1] << "," << p[i][2] << "," << p[i][3] << "]" << std::endl;
	}
	std::cout << "[" << p[3][0] << "," << p[3][1] << "," << p[3][2] << "," << p[3][3] << "]]";
}

float &d_Mat4f::operator () (int ir, int ic)
{ 
	if(ir>4 || ic>4)
	{
		std::cout << "Bad index for matrix!" << std::endl;
		return p[0][0];
	}
	return p[ir][ic]; 
}

float d_Mat4f::operator () (int ir, int ic) const
{ 
	if(ir>4 || ic>4)
	{
		std::cout << "Bad index for matrix!" << std::endl;
		return p[0][0];
	}
	return p[ir][ic]; 
}

d_Vec4f d_Mat4f::operator * (d_Vec4f v)
{
	d_Vec4f result;
	for(int i=0; i<4; i++)
		for(int j=0; j<4; j++)
			result[i] += p[i][j]*v[j];
	return result;
}

d_Mat4f d_Mat4f::operator * (d_Mat4f m)
{
#define N(x,y) p[x][y]
#define M(x,y) m(x,y)
#define R(x,y) result(x,y)

	d_Mat4f result;

	R(0,0) = N(0,0) * M(0,0) + N(0,1) * M(1,0) + N(0,2) * M(2,0) + N(0,3) * M(3,0);
	R(0,1) = N(0,0) * M(0,1) + N(0,1) * M(1,1) + N(0,2) * M(2,1) + N(0,3) * M(3,1);
	R(0,2) = N(0,0) * M(0,2) + N(0,1) * M(1,2) + N(0,2) * M(2,2) + N(0,3) * M(3,2);
	R(0,3) = N(0,0) * M(0,3) + N(0,1) * M(1,3) + N(0,2) * M(2,3) + N(0,3) * M(3,3);
	
	R(1,0) = N(1,0) * M(0,0) + N(1,1) * M(1,0) + N(1,2) * M(2,0) + N(1,3) * M(3,0);
	R(1,1) = N(1,0) * M(0,1) + N(1,1) * M(1,1) + N(1,2) * M(2,1) + N(1,3) * M(3,1);
	R(1,2) = N(1,0) * M(0,2) + N(1,1) * M(1,2) + N(1,2) * M(2,2) + N(1,3) * M(3,2);
	R(1,3) = N(1,0) * M(0,3) + N(1,1) * M(1,3) + N(1,2) * M(2,3) + N(1,3) * M(3,3);
	
	R(2,0) = N(2,0) * M(0,0) + N(2,1) * M(1,0) + N(2,2) * M(2,0) + N(2,3) * M(3,0);
	R(2,1) = N(2,0) * M(0,1) + N(2,1) * M(1,1) + N(2,2) * M(2,1) + N(2,3) * M(3,1);
	R(2,2) = N(2,0) * M(0,2) + N(2,1) * M(1,2) + N(2,2) * M(2,2) + N(2,3) * M(3,2);
	R(2,3) = N(2,0) * M(0,3) + N(2,1) * M(1,3) + N(2,2) * M(2,3) + N(2,3) * M(3,3);
	
	R(3,0) = N(3,0) * M(0,0) + N(3,1) * M(1,0) + N(3,2) * M(2,0) + N(3,3) * M(3,0);
	R(3,1) = N(3,0) * M(0,1) + N(3,1) * M(1,1) + N(3,2) * M(2,1) + N(3,3) * M(3,1);
	R(3,2) = N(3,0) * M(0,2) + N(3,1) * M(1,2) + N(3,2) * M(2,2) + N(3,3) * M(3,2);
	R(3,3) = N(3,0) * M(0,3) + N(3,1) * M(1,3) + N(3,2) * M(2,3) + N(3,3) * M(3,3);
	
	return result;	
	
#undef N
#undef M
#undef R
}

void d_Mat4f::ref(float *tmp)
{
	int k=0;
	//float *tmp = new float[16];
	for(int j=0; j<4; j++)
		for(int i=0; i<4; i++)
		{
			tmp[k]=p[i][j];
			k++;
		}
}

